% IN:
%  skad - [x,y] wspolrzedne punktu z ktorego zaczynamy realizowac sciezke
%  gdzie- [x,y] wsporzedne punktu do ktorego chcemy jechac 
%(jesli bedzie to punkt w korytarzu to robot zatrzyma sie przed korytarzem,
%a jesli poza nim to dojdzie do punktu
% OUT:
%  Zostaje zmieniony parametr klasy: roboty(indeks).droga,
% zostaje tam wpisana tablica punktow ktore nalezy zrealizowac (funkcja
% 'wykonaj ruch' j� realizuje.
% tablica punkt�w ma wymiar 2x48 - wypelnione sa pierwsze pola punktami 
% sciezki a reszta to zera.

%W razie jakichkolwiek problem�w prosz� �mialo pisac :)

function dijkstry (skad,gdzie,indeks,nieprzejezdny)

%skad bedzie zamienione na "roboty(indeks_robota).pozycja"

%wejscie: 
%skad- miejsce z ktorego zaczynamy [x y]
%gdzie-miejsce do ktorego chcemy dojsc [x y]
%funkcja zwraca zmienna globalna LISTA, ktora jest macierza (2, 48)
%z kolejnymi punktami przez ktore przechodzi robot, reszta element�w
%macierzy to "0"

%zmienne:
%global mapa;
global roboty;

krok=0;      % zmienna okre�laj�ca w kt�rym miejscu jeste�my
koniec=0;    % zmienna ktora sprawdzamy czy jest juz koniec 
sciezka=zeros(8,13);    %tablica w ktorej sa min sciezki do w�z��w
minimum=1000;   %zmienna pomocnicza do szukania min w tablicy Q
% Jesli wierzcho�ek jest nieporzetworzony to jest 1 (prawda ze nalezy do Q)
% Macierz S to lista wierzcho�k�w po kt�rych si� poruszamy
% teraz wyr�naimy jeden z wierzcho�k�w, od kt�rego zaczynamy, u nas
% jest to 10

% Tworzymy teraz tablic� w kt�rej b�d� warto�ci d i p
% - Q(1,:) odleglosci do punkt�w
% - Q(2,:) krok w ktorym jestesmy
% - Q(3,:) czy punkt nalzey do Q(1=tak)
Q = [ 1000 1000 1000 1000 1000 1000 1000 1000 1000 0 1000 1000 1000 ; 
         0    0    0    0    0    0    0    0    0 1    0    0    0 ; 
         1    1    1    1    1    1    1    1    1 1    1    1    1 ];
     % wywo�anie funkcji, kt�ra dopisuje punkt pocz�tkowy 
     % (ten w kt�rym jest robot oraz dopisuje jeszcze dwa punkty 

   %mapa=zeros(13);
     
mapa=uzupelnienie_macierzy(skad,gdzie);

%sprawdzenie czy wszytkie puunkty sa przejezdne jesli nie trzeba
%zmodyfikowac jeszcze raz macierz

if (~nieprzejezdny) nieprzejezdny=[0 0]; end;
if ((nieprzejezdny(1)~=0) || (nieprzejezdny(2)~=0))
   mapa=nieprzejezdna_sciezka(mapa,skad,gdzie,nieprzejezdny);
end

%Tworzymy teraz pierwszy punkt, czyli do 10 idziemy tylko przez 10
sciezka(1,10)=10;
%sprawdzamy jeszcze czy nie jestesmy w tym punkcie do ktorego chcemy isc
if (skad==gdzie)
   sciezka(1,13)=10;
   koniec=1; 
end
    
%===================glowna petla algorytmu dijkstry========================

while koniec~=1 % jesli pusty jest zbior Q to koniec

    minimum=1000;   %ustalamy wartosc poczatkow� minimum
    krok=krok+1;
    % Szukamy teraz wiercho�ka o najmniejszym d 
    
    for i=1:13  %wyszukiwanie minimum
        if ((Q(1,i)<minimum) && (Q(3,i)==1))
            minimum=Q(1,i);
            a=i;
        end
    end
    % wpisujemy wierzcholek o najmniejszym d do naszej listy wierzcholkow
    Q(2,a)=krok;
    Q(3,a)=0;
   
    % Teraz sprawdzamy wartosci wszystkich sasiadow danego wierzcholka:
    for i=1:13
        if mapa(a,i)~=0
    % jesli dany punkt ma s�siad�w (miejsca w mapie sa 
    % rozne od zera) to dopisujemy odpowiednie wartosci do macierzy Q
            if (mapa(a,i)+Q(1,a))<Q(1,i)
                Q(1,i)=mapa(a,i)+Q(1,a); %przeniesienie do ze zbioru Q
                %tutaj mamy sasiadow, czyli mozemy napisac najkrotsza
                %sciezke do nich
                %dopisanie kolejnych w�z��w przez kt�re trzeba przej�� �eby
                %mie� najkr�tsz� �cie�k�
                for j=1:13  
                    if (sciezka(j,a)==0)
                    break;
                    end;
                    sciezka(j,i)=sciezka(j,a);
                end
                sciezka(j,i)=i;
            end   
        end
    end
    %====sprawdzanie czy wszystkie punkty juz zostaly zrobione============
    
    %jesli idziemy nie na regal to pomijamy 11 i 12, a reszt� sprawdzamy
    if (mapa(11,13)==0 && mapa (12,13)==0)
        for i=1:10
            if Q(3,i)==1%jesli jest cos z 1 to znaczy ze jeszcze nie koniec 
                koniec=0;%jesli nie ma nigdzie 1 to koniec algorytmu
                break;
            else 
                koniec=1;
            end
        end       
        %jeszcze zostaje punkt 13 do sprawdzenia czy zosta juz zrobiony
        if (Q(3,13)==1) 
            koniec=0;
        end
        
    %jesli idziemy na regal to sprawdzamy wszystkie punkty    
    else 
        
    
        for i=1:13
            if Q(3,i)==1%jesli jest cos z 1 to znaczy ze jeszcze nie koniec 
                koniec=0;%jesli nie ma nigdzie 1 to koniec algorytmu
                break;
            else 
                koniec=1;
            end
        end
    end

 
end %=============koniec while, zbi�r Q jest pusty=========================

%funkcja tworz�ca zmienna globalna LISTA na odstawie punkt�w ze zmiennej
%sciezka

lista=wypisanie_sciezki(sciezka,skad,gdzie);
roboty(indeks).droga=lista;
i=1;
while (~isequal(roboty(indeks).droga(i,:),[0 0]))		
    i=i+1;		
end;		
roboty(indeks).cel = roboty(indeks).droga(i-1,:);


